06. Solution: Global Kinematic Model

The solution below uses the Eigen library, state and actuator vector inputs, and the equations for the next state (below), to implement the Global Kinematic Model.

x_{t+1} = x_t + v_t * cos(\psi_t) * dt
y_{t+1} = y_t + v_t * sin(\psi_t) * dt
\psi_{t+1} = \psi_t + \frac {v_t} { L_f} * \delta * dt
v_{t+1} = v_t + a_t * dt

Start Quiz:

// In this quiz you'll implement the global kinematic model.
#include <math.h>
#include <iostream>
#include "Dense"

//
// Helper functions
//
double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }

const double Lf = 2;


Eigen::VectorXd globalKinematic(Eigen::VectorXd state,
                                Eigen::VectorXd actuators, double dt) {
  Eigen::VectorXd next_state(state.size());

  //The next_state calculation ...
  
  next_state[0] = state[0] + state[3] * cos(state[2])*dt;
  next_state[1] = state[1] + state[3] * sin(state[2])*dt;
  next_state[2] = state[2] + state[3]/Lf *actuators[0]*dt;
  next_state[3] = state[3] + actuators[1]*dt;

  return next_state;
}

int main() {
  // [x, y, psi, v]
  Eigen::VectorXd state(4);
  // [delta, v]
  Eigen::VectorXd actuators(2);

  state << 0, 0, deg2rad(45), 1;
  actuators << deg2rad(5), 1;

  Eigen::VectorXd next_state = globalKinematic(state, actuators, 0.3);

  std::cout << next_state << std::endl;
}

INSTRUCTOR NOTE:

Code and files for running this quiz locally can be found here.